#pragma once
#include <rclcpp/rclcpp.hpp>
#include <string>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <tf2/LinearMath/Transform.h>
#include <geometry_msgs/msg/pose.hpp>
#include <geometry_msgs/msg/transform_stamped.hpp>

/**
 * @brief PoseInfo结构体, 用于存储位姿信息
 * @details 包含父坐标系ID, 子坐标系ID, 位置(x, y, z)和四元数(qx, qy, qz, qw)
 * @details 支持到ROS消息类型的转换, 包括Pose和TransformStamped
 * @note 该结构体提供了基本的算术运算符重载(减法)以及计算范数的方法
 */
struct PoseInfo {
    PoseInfo(std::string father_frame_id = "", std::string child_frame_id = ""):
        father_frame_id(father_frame_id), child_frame_id(child_frame_id),
        point{ 0.0, 0.0, 0.0 }, quaternion{ 0.0, 0.0, 0.0, 1.0 } { }
    PoseInfo(
        std::string father_frame_id, std::string child_frame_id,
        double x, double y, double z,
        double qx, double qy, double qz, double qw
    ):
        father_frame_id(father_frame_id),
        child_frame_id(child_frame_id),
        point{x, y, z},
        quaternion{qx, qy, qz, qw}
        { }
    PoseInfo(
        std::string father_frame_id, std::string child_frame_id,
        geometry_msgs::msg::Pose& pose
    ):
        father_frame_id(father_frame_id),
        child_frame_id(child_frame_id),
        point{pose.position.x, pose.position.y, pose.position.z},
        quaternion{pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w}
        { }
    PoseInfo(
        std::string father_frame_id, std::string child_frame_id,
        tf2::Transform& transform
    ):
        father_frame_id(father_frame_id),
        child_frame_id(child_frame_id),
        point{transform.getOrigin().x(), transform.getOrigin().y(), transform.getOrigin().z()},
        quaternion{transform.getRotation().x(), transform.getRotation().y(), transform.getRotation().z(), transform.getRotation().w()}
        { }
    PoseInfo operator-(const PoseInfo& other) const {
        auto q1 = to_tf_quaternion();
        auto q2 = other.to_tf_quaternion();
        auto q_diff = q1 * q2.inverse();  // 从other到this的坐标系变换
        return PoseInfo(
            father_frame_id, child_frame_id,
            point.x - other.point.x, point.y - other.point.y, point.z - other.point.z,
            q_diff.x(), q_diff.y(), q_diff.z(), q_diff.w()
        );
    }
    std::pair<double, double> norm() const {
        double dist_norm = std::sqrt(point.x * point.x + point.y * point.y);
        auto q = to_tf_quaternion();
        double row, pitch, yaw;
        tf2::Matrix3x3(q).getRPY(row, pitch, yaw);
        double yaw_norm = std::abs(yaw);
        return {dist_norm, yaw_norm};
    }
    
    geometry_msgs::msg::Pose to_pose_msg() {
        geometry_msgs::msg::Pose pose;
        pose.position.x = point.x;
        pose.position.y = point.y;
        pose.position.z = point.z;
        pose.orientation.x = quaternion.x;
        pose.orientation.y = quaternion.y;
        pose.orientation.z = quaternion.z;
        pose.orientation.w = quaternion.w;
        return pose;
    }

    geometry_msgs::msg::TransformStamped to_tf_msg() {
        geometry_msgs::msg::TransformStamped tf_msg;
        tf_msg.header.stamp = rclcpp::Time(0, 0);
        tf_msg.header.frame_id = father_frame_id;
        tf_msg.child_frame_id = child_frame_id;
        tf_msg.transform.translation.x = point.x;
        tf_msg.transform.translation.y = point.y;
        tf_msg.transform.translation.z = point.z;
        tf_msg.transform.rotation.x = quaternion.x;
        tf_msg.transform.rotation.y = quaternion.y;
        tf_msg.transform.rotation.z = quaternion.z;
        tf_msg.transform.rotation.w = quaternion.w;
        return tf_msg;
    }

    tf2::Vector3 to_tf_vector() const {
        return tf2::Vector3(point.x, point.y, point.z);
    }

    tf2::Quaternion to_tf_quaternion() const {
        return tf2::Quaternion(quaternion.x, quaternion.y, quaternion.z, quaternion.w);
    }

    tf2::Transform to_tf_transform() const {
        tf2::Transform transform;
        transform.setOrigin(to_tf_vector());
        transform.setRotation(to_tf_quaternion());
        return transform;
    }

    std::string father_frame_id;
    std::string child_frame_id;
    struct Point {
        double x, y, z;
    } point;
    struct Quaternion {
        double x, y, z, w;
    } quaternion;
};
